//
// Created by lqk on 2022/2/21.
//

#include "PyBulletCheaterSensorDriver.h"

CheaterSensorInfo g_cheaterInfo;

void PyBulletCheaterSensorDriver::Init() {
    if(!m_lcmSub.good()) std::cout <<"LCM subscriber not good!"<<std::endl;
    m_lcmSub.subscribe(m_subChannel.c_str(),&PyBulletCheaterSensorDriver::Callback,this);
}

void PyBulletCheaterSensorDriver::RunOnce() {
    m_lcmSubHandle = m_lcmSub.handleTimeout(500);
}

void PyBulletCheaterSensorDriver::CleanUp() {

}

void PyBulletCheaterSensorDriver::Callback(const lcm::ReceiveBuffer *rbuf, const std::string &channel,
                                 const quad_bot::cheater_sensor_info_pb *msg) {
    for(int i=0;i<3;i++){
        g_cheaterInfo.m_pos[i]=msg->pos[i];
        g_cheaterInfo.m_vel[i]=msg->vel[i];
        g_cheaterInfo.m_omega[i]=msg->omega[i];
    }
    g_cheaterInfo.m_quat.x()=msg->quat[0];
    g_cheaterInfo.m_quat.y()=msg->quat[1];
    g_cheaterInfo.m_quat.z()=msg->quat[2];
    g_cheaterInfo.m_quat.w()=msg->quat[3];
//    std::cout<<m_name<<" callback"<<std::endl;
//    for(int i=0;i<3;i++){
//        std::cout<<msg->pos[i]<<" ";
//    }
//    for(int i=0;i<3;i++){
//        std::cout<<msg->vel[i]<<" ";
//    }
//    for(int i=0;i<3;i++){
//        std::cout<<msg->omega[i]<<" ";
//    }
//    for(int i=0;i<4;i++){
//        std::cout<<msg->quat[i]<<" ";
//    }
//    std::cout<<std::endl;


}

void PyBulletCheaterSensorDriver::PrintStatus() {
//    std::cout<<m_name<<" is running!"<<std::endl;
    if(m_lcmSubHandle==0){
        std::cout<<m_name<<" subscription time out!"<<std::endl;
    }
}